%pylab inline
import os
import cv2
import numpy as np
pylab.rcParams['figure.figsize'] = (14, 12)
def bgr2rgb(img):
return cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
def plot_images(images, row=1, im_prep=None, titles=None, cmap=None):
add_col = 1 if (len(images) % row) else 0
col = (len(images) // row) + add_col
fig, axes = subplots(row,col, subplot_kw={'xticks': [], 'yticks': []})
fig.subplots_adjust(hspace=0.3, wspace=0.05)
if im_prep is None:
im_prep = lambda x: x
for i, ax in enumerate(axes.flat):
if i >= len(images):
break
img = im_prep(images[i])
if (len(img.shape) < 3) and cmap is None:
cmap='gray'
ax.imshow(img, cmap=cmap)
if titles is not None:
ax.set_title(titles[i])
show()
camera_cal = 'camera_cal/'
chessboard_images = []
for filename in os.listdir(camera_cal):
image = cv2.imread(camera_cal + filename)
if image is not None:
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
chessboard_images.append(image)
print(len(chessboard_images))
print(imread(camera_cal+filename).shape)
plot_images(chessboard_images, row=5)
chessboard_image = imread('camera_cal/calibration1.jpg')
gray = cv2.cvtColor(chessboard_image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (9, 5), None)
# If found, draw corners
if ret == True:
img = cv2.drawChessboardCorners(chessboard_image, (9,5), corners, ret)
imshow(img)
objectPoints – In the new interface it is a vector of vectors of calibration pattern points in the calibration pattern coordinate space (e.g. std::vectorstd::vector<cv::Vec3f>). The outer vector contains as many elements as the number of the pattern views. If the same calibration pattern is shown in each view and it is fully visible, all the vectors will be the same. Although, it is possible to use partially occluded patterns, or even different patterns in different views. Then, the vectors will be different. The points are 3D, but since they are in a pattern coordinate system, then, if the rig is planar, it may make sense to put the model to a XY coordinate plane so that Z-coordinate of each input object point is 0.
In the old interface all the vectors of object points from different views are concatenated together.
imagePoints – In the new interface it is a vector of vectors of the projections of calibration pattern points (e.g. std::vectorstd::vector<cv::Vec2f>). imagePoints.size() and objectPoints.size() and imagePoints[i].size() must be equal to objectPoints[i].size() for each i.
In the old interface all the vectors of object points from different views are concatenated together.
# number of corners nx & ny
n_corners = (9, 6)
nx = n_corners[0]
ny = n_corners[1]
# for the meth cv2.calibraceCamera we need objpoints
objpoints = []
objp = np.zeros((nx*ny, 3), np.float32)
objp[:,:2] = np.mgrid[0:nx,0:ny].T.reshape(-1,2)
imgpoints = []
for gray in chessboard_images:
ret, corners = cv2.findChessboardCorners(gray, n_corners, None)
# If found, add to imgpoints
if ret == True:
imgpoints.append(corners)
objpoints.append(objp)
print(len(imgpoints))
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape, None, None)
img = chessboard_images[0]
dst = cv2.undistort(img, mtx, dist, None, mtx)
#imshow(np.hstack((img, dst)), cmap='gray')
plot_images((img, dst), cmap='gray', im_prep=None, titles=('Original', 'Undistored'))
def undistort(img):
return cv2.undistort(img, mtx, dist, None, mtx)
mtx
dist
img = cv2.imread('test_images/test1.jpg')
plot_images((img, undistort(img)), im_prep=bgr2rgb)
def corners_unwarp(img, nx, ny):
# remove distortion
undist = undistort(img)
# Convert undistorted image to grayscale
gray = cv2.cvtColor(undist, cv2.COLOR_BGR2GRAY)
# Search for corners in the grayscaled image
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
if ret == True:
# If we found corners, draw them! (just for fun)
cv2.drawChessboardCorners(undist, (nx, ny), corners, ret)
# Choose offset from image corners to plot detected corners
# This should be chosen to present the result at the proper aspect ratio
# My choice of 100 pixels is not exact, but close enough for our purpose here
offset = 100 # offset for dst points
# Grab the image shape
img_size = (gray.shape[1], gray.shape[0])
# For source points I'm grabbing the outer four detected corners
src = np.float32([corners[0], corners[nx-1], corners[-1], corners[-nx]])
# For destination points, I'm arbitrarily choosing some points to be
# a nice fit for displaying our warped result
# again, not exact, but close enough for our purposes
dst = np.float32([[offset, offset], [img_size[0]-offset, offset],
[img_size[0]-offset, img_size[1]-offset],
[offset, img_size[1]-offset]])
# Given src and dst points, calculate the perspective transform matrix
M = cv2.getPerspectiveTransform(src, dst)
# Warp the image using OpenCV warpPerspective()
warped = cv2.warpPerspective(undist, M, img_size)
# Return the resulting image and matrix
return warped, M
def transform(undist_img, matrix):
cv2.warpPerspective(undist_img, matrix, undist_img.shape[-2::-1])
img = cv2.imread('camera_cal/calibration12.jpg')
dst = undistort(img)
warp, matrix = corners_unwarp(img, nx, ny)
plot_images((img, warp))
corners_int = np.int32([[190,720],[584,457],[700,457],[1126,720]])
def getMatrix():
corners = np.float32(corners_int)
new_top_left=np.array([corners[0,0],0])
new_top_right=np.array([corners[3,0],0])
offset=[200,0]
src = np.float32([corners[0],corners[1],corners[2],corners[3]])
dst = np.float32([corners[0]+offset,new_top_left+offset,new_top_right-offset,corners[3]-offset])
M = cv2.getPerspectiveTransform(src, dst)
return M
camera_M = getMatrix()
def warp_camera(img, inv=False):
if len(img.shape)==3:
img_size = img.shape[-2::-1]
else:
img_size = img.shape[::-1]
flags = cv2.INTER_LINEAR + cv2.WARP_FILL_OUTLIERS
if inv:
flags += cv2.WARP_INVERSE_MAP
return cv2.warpPerspective(img, camera_M, img_size, flags=flags)
camera_M
camera_img = cv2.imread('test_images/straight_lines1.jpg')
undist = undistort(camera_img)
cv2.polylines(undist, [corners_int], True, [0,255,0], 2)
warp = warp_camera(undist)
plot_images((undist, warp, warp_camera(warp, True)), titles=('Original', 'Transformed', 'Unwarped'), im_prep=bgr2rgb, row=3)
print(camera_img.shape)
print(warp.shape)
def transform(img, inv=False):
"""
returns undistorted and warped img
"""
undist = undistort(img)
warp = warp_camera(undist, inv)
return warp
camera_img = cv2.imread('test_images/test2.jpg')
plot_images((camera_img, transform(camera_img)), titles=('Original', 'Transformed'), im_prep=bgr2rgb)
print(camera_img.shape)
def region_of_interest(img):
"""
Applies an image mask on warped image
Only keeps the region of the image defined by the polygon
formed from `vertices`. The rest of the image is set to black.
"""
h,w = img.shape[:2]
reg_bl = [220, h]
reg_br = [w-220, h]
reg_tl = [100, 0]
reg_tr = [w-100, 0]
vertices = np.array([[reg_bl, reg_tl, reg_tr, reg_br]], dtype=np.int32)
#defining a blank mask to start with
mask = np.zeros_like(img)
#defining a 3 channel or 1 channel color to fill the mask with depending on the input image
if len(img.shape) > 2:
channel_count = img.shape[2] # i.e. 3 or 4 depending on your image
ignore_mask_color = (255,) * channel_count
else:
ignore_mask_color = 255
#filling pixels inside the polygon defined by "vertices" with the fill color
cv2.fillPoly(mask, vertices, ignore_mask_color)
#returning the image only where mask pixels are nonzero
masked_image = cv2.bitwise_and(img, mask)
return masked_image
camera_img = cv2.imread('test_images/test2.jpg')
transf = transform(camera_img)
plot_images((camera_img, transf, region_of_interest(transf)), titles=('Original','Transformed', 'Region'), im_prep=bgr2rgb)
print(camera_img.shape)
img = cv2.imread('test_images/test2.jpg')
#img = cv2.imread('output_images/c595.jpg')
transf = transform(region_of_interest(img))
plot_images((img, transf), titles=('Original', 'Transformed'), im_prep=bgr2rgb)
print(camera_img.shape)
prep_transf = transf
hls = cv2.cvtColor(prep_transf, cv2.COLOR_BGR2HLS)
hsv = cv2.cvtColor(prep_transf, cv2.COLOR_BGR2HSV)
plot_images((hls[:,:,1], hls[:,:,2], hsv[:,:,1], hsv[:,:,2]),
titles=('hLs', 'hlS', 'hSv', 'hsV'))
def ls_threshold(img, l_thresh_min=40, s_thresh_min=100):
hls = cv2.cvtColor(transf, cv2.COLOR_BGR2HLS)
l = hls[:,:,1]
s = hls[:,:,2]
# l_bin = np.zeros_like(l, dtype=np.ubyte)
# l_bin[l>l_thresh_min] = 255
# s_bin = np.zeros_like(s, dtype=np.ubyte)
# s_bin[s>s_thresh_min] = 255
ret = np.zeros_like(s, dtype=np.ubyte)
ret[(l>l_thresh_min) & (s>s_thresh_min)] = 255
return ret
plot_images((bgr2rgb(prep_transf), ls_threshold(prep_transf)))
def abs_sobel_threshold(img, orient='x', sobel_kernel=3, thresh=(20, 100)):
# sobel threshold
dx = 1 if orient == 'x' else 0
dy = 1 if orient != 'x' else 0
sobel = cv2.Sobel(gray, cv2.CV_64F, dx, dy, ksize=sobel_kernel)
abs_sobel = np.absolute(sobel)
scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
thresh_min = thresh[0]
thresh_max = thresh[1]
sxbinary = np.zeros_like(scaled_sobel)
sxbinary[(scaled_sobel >= thresh_min) & (scaled_sobel <= thresh_max)] = 1
return sxbinary
def mag_thresheld(image, sobel_kernel=3, mag_thresh=(20, 100)):
# Convert to grayscale
gray = image
# Take both Sobel x and y gradients
sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# Calculate the gradient magnitude
gradmag = np.sqrt(sobelx**2 + sobely**2)
# Rescale to 8 bit
scale_factor = np.max(gradmag)/255
gradmag = (gradmag/scale_factor).astype(np.uint8)
# Create a binary image of ones where threshold is met, zeros otherwise
binary_output = np.zeros_like(gradmag)
binary_output[(gradmag >= mag_thresh[0]) & (gradmag <= mag_thresh[1])] = 1
# Return the binary image
return binary_output
def dir_threshold(image, sobel_kernel=3, thresh=(0, np.pi/2)):
# Grayscale
gray = image
# Calculate the x and y gradients
sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# Take the absolute value of the gradient direction,
# apply a threshold, and create a binary image result
absgraddir = np.arctan2(np.absolute(sobely), np.absolute(sobelx))
binary_output = np.zeros_like(absgraddir)
binary_output[(absgraddir >= thresh[0]) & (absgraddir <= thresh[1])] = 1
# Return the binary image
return binary_output
gray = cv2.cvtColor(prep_transf, cv2.COLOR_BGR2GRAY)
gradx = abs_sobel_threshold(gray, 'x', 5, thresh=(30, 100))
grady = abs_sobel_threshold(gray, 'y', 15, thresh=(20, 100))
mg_thresh = mag_thresheld(gray, sobel_kernel=5, mag_thresh=(15, 70))
dir_thresh = dir_threshold(gray, sobel_kernel=15, thresh=(-0.1, .8))
plot_images((bgr2rgb(transf), gray, gradx, grady, mg_thresh, dir_thresh), row=3,
titles=('original','Gray', 'sobelx', 'sobely', 'mag thresh', 'dir'))
gradxy = np.zeros_like(dir_thresh)
gradxy[((gradx == 1) & (grady != 1)) ] = 255
dir_mag = np.zeros_like(dir_thresh)
dir_mag[((mg_thresh == 1) & (dir_thresh == 1))] = 255
combined = np.zeros_like(dir_thresh)
combined[((gradx == 1) & (grady != 1)) | ((mg_thresh == 1) & (dir_thresh == 1))] = 255
plot_images((gradxy, dir_mag, combined), titles=('gradx & !grady', 'mg_thresh & dir_thresh', 'combined'))
ls_thresh = ls_threshold(prep_transf)
binary = np.zeros_like(ls_thresh)
binary[(ls_thresh==255) | (combined==255)] = 255
binary = region_of_interest(binary)
plot_images((bgr2rgb(transf),binary))
histogram = np.sum(binary[binary.shape[0]//2:,:], axis=0) # lower half of the image
plot(histogram)
# Take a histogram of the bottom half of the image
binary_warped = binary
halfh = binary_warped.shape[0]//2
histogram = np.sum(binary_warped[halfh:,:], axis=0)
out_img = np.dstack((binary_warped, binary_warped, binary_warped))
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = histogram.shape[0]//2
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
# Choose the number of sliding windows
nwindows = 10
# Set height of the windows
window_height = binary_warped.shape[0]//nwindows
# Identify the x and y positions of all nonzero pixels in the image
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Current positions to be updated for each window
leftx_current = leftx_base
rightx_current = rightx_base
# Set the width of the windows +/- margin
margin = 120
# Set minimum number of pixels found to recenter window
minpix = 50
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = binary_warped.shape[0] - (window+1)*window_height
win_y_high = binary_warped.shape[0] - window*window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(0,255,0), 2)
cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 2)
# Identify the nonzero pixels in x and y within the window
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# If you found > minpix pixels, recenter next window on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
imshow(out_img)
plot(left_fitx, ploty, color='yellow')
plot(right_fitx, ploty, color='yellow')
xlim(0, 1280)
ylim(720, 0)
# in pixels
y_eval = np.max(ploty)
left_curverad = ((1 + (2*left_fit[0]*y_eval + left_fit[1])**2)**1.5) / np.absolute(2*left_fit[0])
right_curverad = ((1 + (2*right_fit[0]*y_eval + right_fit[1])**2)**1.5) / np.absolute(2*right_fit[0])
print(left_curverad, ',', right_curverad)
# Define conversions in x and y from pixels space to meters
ym_per_pix = 30/720 # meters per pixel in y dimension
xm_per_pix = 3.7/700 # meters per pixel in x dimension
# Fit new polynomials to x,y in world space
left_fit_cr = np.polyfit(lefty*ym_per_pix, leftx*xm_per_pix, 2)
right_fit_cr = np.polyfit(righty*ym_per_pix, rightx*xm_per_pix, 2)
# Calculate the new radii of curvature
left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])
# Now our radius of curvature is in meters
print(left_curverad, 'm,', right_curverad, 'm')
# Create an image to draw on and an image to show the selection window
out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
window_img = np.zeros_like(out_img)
# Color in left and right line pixels
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
# Generate a polygon to illustrate the search window area
# And recast the x and y points into usable format for cv2.fillPoly()
left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, ploty])))])
left_line_pts = np.hstack((left_line_window1, left_line_window2))
right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, ploty])))])
right_line_pts = np.hstack((right_line_window1, right_line_window2))
# Draw the lane onto the warped blank image
cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
imshow(result)
plot(left_fitx, ploty, color='yellow')
plot(right_fitx, ploty, color='yellow')
xlim(0, 1280)
ylim(720, 0)
# Create an image to draw the lines on
warped = binary
color_warp = np.zeros((*warped.shape, 3), dtype=np.ubyte)
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
# Warp the blank back to original image space using inverse perspective matrix (Minv)
undist = undistort(img)
newwarp = transform(color_warp, True)
# Combine the result with the original image
result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)
imshow(bgr2rgb(result))
from scipy.misc import imresize
def overlay_image(img, s_img, pos, size=None):
x_offset,y_offset=pos
l_img = img.copy()
if size is not None:
s_img = imresize(s_img, size)
if len(s_img.shape) == 2:
s_img = cv2.merge((s_img,s_img,s_img))
l_img[y_offset:y_offset+s_img.shape[0], x_offset:x_offset+s_img.shape[1]] = s_img
return l_img
ret = overlay_image(result, prep_transf, (0,0), (200, 400))
ret = overlay_image(ret, warped, (402, 0), (200, 400))
ret = overlay_image(ret, out_img, (804, 0), (200, 400))
imshow(bgr2rgb(ret))
# measuring the offset of the car from the lane's middle
lb = np.average(left_fitx[-10:])
rb = np.average(right_fitx[-10:])
lane_mid = (rb-lb)/2 + lb
middle = binary_warped.shape[1]/2
print(lane_mid, middle)
xm_per_pix = 3.7 / 700 # meters per pixel in x dimension
print("car offset is", abs(lane_mid-middle)*xm_per_pix, 'm')